FUNCTION_BLOCK proc_state
  VAR_INPUT
    ERR_HOME : BOOL;
    ERR_HOME_ID : WORD;
    ERR_MOVE : BOOL;
    ERR_MOVE_ID : WORD;
    ERR_MOVE1 : BOOL;
    ERR_MOVE2 : BOOL;
    HOME : BOOL;
    ACT_MOVE : BOOL;
    ACT_MOVE0 : BOOL;
  END_VAR
  VAR_OUTPUT
    OUT : INT;
  END_VAR

  OUT:=0;

  if HOME then
  OUT:=1;
  end_if;

  if ACT_MOVE then
  OUT:=2;
  end_if;

  if ERR_MOVE then
  OUT:=-WORD_TO_INT(ERR_MOVE_ID);
  end_if;

  if ERR_HOME then
  OUT:=-WORD_TO_INT(ERR_HOME_ID);
  end_if;
END_FUNCTION_BLOCK

FUNCTION_BLOCK stepper
  VAR_INPUT
    AXIS : AXIS_REF;
  END_VAR
  VAR_OUTPUT
    POS : REAL;
    STATE : INT;
  END_VAR
  VAR_INPUT
    TARGET : REAL;
  END_VAR
  VAR_IN_OUT
    RUN : BOOL;
  END_VAR
  VAR_OUTPUT
    RUNING : BOOL;
  END_VAR
  VAR_INPUT
    SPEED : REAL;
    ASPEED : REAL;
  END_VAR
  VAR_IN_OUT
    HOME : BOOL;
  END_VAR
  VAR_INPUT
    HOMESPEED : REAL;
    HOMELOOKUP : REAL;
    XIFEN : REAL;
    RUN_DIR : MC_DIRECTION;
    HOMEMODE : MC_HOMING_PROCEDURES;
  END_VAR
  VAR
    BUFFMODE : MC_BUFFER_MODE := mcAborting;
    MC_SetPosMODE : MC_EXECUTION_MODE := mcImmediately;
    MC_WriteParameter8 : MC_WriteParameter;
    MC_Home1 : MC_Home;
    MC_MoveAbsolute3 : MC_MoveAbsolute;
    MC_Power3 : MC_Power;
    MC_ReadActualPosition3 : MC_ReadActualPosition;
    proc_state3 : proc_state;
    MC_SetPosition3 : MC_SetPosition;
    MC_WriteParameter0 : MC_WriteParameter;
    MC_WriteParameter6 : MC_WriteParameter;
    _TMP_MOVE138_ENO : BOOL;
    _TMP_MOVE138_OUT : BOOL;
    _TMP_OR222_OUT : BOOL;
    _TMP_DIV167_OUT : REAL;
  END_VAR

  MC_ReadActualPosition3(Axis := AXIS, Enable := MC_Power3.Status);
  POS := MC_ReadActualPosition3.Position;
  _TMP_MOVE138_OUT := MOVE(EN := MC_MoveAbsolute3.Done, IN := 0, ENO => _TMP_MOVE138_ENO);
  IF _TMP_MOVE138_ENO THEN
      RUN := _TMP_MOVE138_OUT;
  END_IF;
  proc_state3(ERR_HOME := MC_Home1.Error, ERR_HOME_ID := MC_Home1.ErrorID, ERR_MOVE := MC_MoveAbsolute3.Error, HOME := MC_Home1.Active, ACT_MOVE := MC_MoveAbsolute3.Active);
  STATE := proc_state3.OUT;
  _TMP_OR222_OUT := OR(MC_MoveAbsolute3.Busy, MC_Home1.Busy);
  RUNING := _TMP_OR222_OUT;
  MC_Power3(Axis := AXIS, Enable := 1, EnablePositive := 1, EnableNegative := 1);
  MC_Home1(Axis := AXIS, Execute := MC_WriteParameter0.Done, HomingMode := HOMEMODE, BufferMode := BUFFMODE);
  MC_SetPosition3(Axis := AXIS, Execute := MC_Home1.Done, Position := 0.0, ExecutionMode := MC_SetPosMODE);
  MC_MoveAbsolute3(Axis := AXIS, Execute := RUN, ContinuousUpdate := 1, Position := TARGET, Velocity := SPEED, Acceleration := _TMP_DIV167_OUT, Deceleration := _TMP_DIV167_OUT, Direction := RUN_DIR);
  _TMP_DIV167_OUT := DIV(SPEED, ASPEED);
  MC_WriteParameter8(Axis := AXIS, Execute := HOME, ParameterNumber := 1018, Value := HOMELOOKUP);
  MC_WriteParameter0(Axis := AXIS, Execute := MC_WriteParameter8.Done, ParameterNumber := 1010, Value := HOMESPEED);
  MC_WriteParameter6(Axis := AXIS, Execute := MC_Power3.Status, ParameterNumber := 1003, Value := XIFEN);
END_FUNCTION_BLOCK

FUNCTION_BLOCK WK_HYP
  VAR_INPUT
    H : REAL;
    CV : REAL;
    L : REAL;
  END_VAR
  VAR_OUTPUT
    OUT : UINT;
  END_VAR
  VAR
    OUT0 : UINT;
    COUNT : UINT;
  END_VAR

  IF CV>=H AND COUNT>50 THEN
    OUT0:=0;
    COUNT:=0;
  END_IF;
    
  IF CV<L AND COUNT>50 THEN
    OUT0:=100;
    COUNT:=0;
  END_IF;

  OUT:=OUT0;
  COUNT:=COUNT+1;
END_FUNCTION_BLOCK

FUNCTION_BLOCK WK_PID
  VAR_INPUT
    SV : REAL;
    CV : REAL;
    RANGER : REAL;
    KP : REAL;
    KI : REAL;
    KD : REAL;
  END_VAR
  VAR_OUTPUT
    OUT : REAL;
    PIDING : BOOL;
  END_VAR
  VAR
    ERROR : REAL;
    SumError : REAL;
    DERROR : REAL;
    PERROR : REAL;
    LERROR : REAL;
  END_VAR

  ERROR:=SV-CV;

  if OUT>0.0 and OUT<100.0 then
   SumError:=SumError+ERROR;
  end_if;


    
  DERROR:=LERROR-PERROR;
  PERROR:=LERROR;
  LERROR:=ERROR;
  OUT:=KP*ERROR+SumError*KI+DERROR*KD;

  IF OUT>100.0 THEN
    OUT:=100.0;
  ELSE  
    IF OUT<0.0 THEN
      OUT:=0.0;
    END_IF;
  END_IF;

  IF CV>(SV+RANGER) THEN
    OUT:=0.0;
  ELSE 
    IF CV<(SV-RANGER) THEN
      OUT:=100.0;
    END_IF;
  END_IF;
END_FUNCTION_BLOCK

PROGRAM program0
  VAR
    reset_all : BOOL;
    gohome : BOOL;
    WKON1 : BOOL := 0;
    WKON2 : BOOL := 0;
    WKON3 : BOOL := 0;
    WKON4 : BOOL := 0;
  END_VAR
  VAR RETAIN
    WKSET1 : REAL := 20.0;
    WKSET2 : REAL := 20.0;
    WKSET3 : REAL := 20.0;
    WKSET4 : REAL := 20.0;
  END_VAR
  VAR
    WKSET_L : REAL := 5.0;
    TEMP1 : REAL;
    TEMP2 : REAL;
    TEMP3 : REAL;
    TEMP4 : REAL;
  END_VAR
  VAR RETAIN
    JIAOZHUN1 : REAL := 0.0;
    JIAOZHUN2 : REAL := 0.0;
    JIAOZHUN3 : REAL := 0.0;
    JIAOZHUN4 : REAL := 0.0;
  END_VAR
  VAR
    TEMP_IN1 AT %ID1.12.0 : REAL;
    TEMP_IN2 AT %ID1.12.1 : REAL;
    TEMP_IN3 AT %ID1.12.2 : REAL;
    TEMP_IN4 AT %ID1.12.3 : REAL;
    LocalVar14 AT %IX1.10.5 : BOOL;
    LocalVar15 AT %IX1.10.6 : BOOL;
    LocalVar16 AT %IX1.10.7 : BOOL;
    LocalVar17 AT %IX1.10.8 : BOOL;
  END_VAR
  VAR
    SPEED1 : REAL := 1.0;
    ASPEED1 : REAL := 0.5;
    SPEED2 : REAL := 1.0;
    ASPEED2 : REAL := 0.5;
    SPEED3 : REAL := 1.0;
    ASPEED3 : REAL := 0.5;
    SPEED4 : REAL := 1.0;
    ASPEED4 : REAL := 0.5;
    TARGET1 : REAL := 10.0;
    POS1 : REAL := 0.0;
    TARGET2 : REAL := 10.0;
    POS2 : REAL := 20.0;
    TARGET3 : REAL := 10.0;
    POS3 : REAL := 30.0;
    TARGET4 : REAL := 10.0;
    POS4 : REAL := 30.0;
    STATUS1 : INT := -1;
    STATUS2 : INT := -2;
    STATUS3 : INT := -3;
    STATUS4 : INT := -4;
    RUN1 : BOOL;
    RUN2 : BOOL;
    RUN3 : BOOL;
    RUN4 : BOOL;
  END_VAR
  VAR RETAIN
    HOMESPEED1 : REAL := 2.0;
    HOMESPEED2 : REAL := 2.0;
    HOMESPEED3 : REAL := 2.0;
    HOMESPEED4 : REAL := 2.0;
    HOMESPEEDZERO1 : REAL := 0.2;
    HOMESPEEDZERO2 : REAL := 0.2;
    HOMESPEEDZERO3 : REAL := 0.2;
    HOMESPEEDZERO4 : REAL := 0.2;
  END_VAR
  VAR
    HOME1 : BOOL;
    HOME2 : BOOL;
    HOME3 : BOOL;
    HOME4 : BOOL;
  END_VAR
  VAR
    DCMOTOR_RUN AT %QX1.10.25 : BOOL;
    DCMOTOR_DIR AT %QX1.10.26 : BOOL;
    DCMOTOR_SPEED_SET AT %QW1.11.16 : UINT;
    DCMOTOR_SPEED AT %IW1.11.14 : UINT;
  END_VAR
  VAR RETAIN
    DCMOTOR_FREQ AT %QW1.11.15 : UINT := 20000;
  END_VAR
  VAR
    DC_MOTOR2_SPEED AT %QW1.11.17 : INT;
    J1 AT %QX1.10.16 : BOOL;
    J2 AT %QX1.10.17 : BOOL;
    J3 AT %QX1.10.18 : BOOL;
    J4 AT %QX1.10.19 : BOOL;
  END_VAR
  VAR
    RUN_EN : BOOL := 1;
    TEMPUSE1 : BOOL := 1;
    MC_Home0 : MC_Home;
    AXIS_0 : AXIS_REF := 0;
    AXIS_1 : AXIS_REF := 1;
    AXIS_2 : AXIS_REF := 2;
    AXIS_3 : AXIS_REF := 3;
    MC_CamTableSelect1 : MC_CAM_REF;
    WK_PID0 : WK_PID;
  END_VAR
  VAR RETAIN
    KP : REAL := 0.1;
    KI : REAL := 0.001;
    KD : REAL := 0.0;
  END_VAR
  VAR
    PWM1 AT %QW1.11.4 : UINT;
    PWM2 AT %QW1.11.5 : UINT;
    PWM3 AT %QW1.11.6 : UINT;
    PWM4 AT %QW1.11.7 : UINT;
  END_VAR
  VAR
    XIFEN1 : REAL := 2560.0;
    XIFEN2 : REAL := 200.0;
    XIFEN3 : REAL := 200.0;
    XIFEN4 : REAL := 200.0;
    MC_MoveAbsolute0 : MC_MoveAbsolute;
    MC_Power0 : MC_Power;
    HOME_P : MC_HOMING_PROCEDURES := mcHomeAbsoluteSwitchPositive;
    HOME_N : MC_HOMING_PROCEDURES := mcHomeAbsoluteSwitchNegative;
    DIR_P : MC_DIRECTION := mcPositiveDirection;
    DIR_N : MC_DIRECTION := mcShortestWay;
    BUFFMODE : MC_BUFFER_MODE := mcAborting;
  END_VAR
  VAR
    X1 AT %IX1.10.1 : BOOL;
    X2 AT %IX1.10.2 : BOOL;
    X3 AT %IX1.10.3 : BOOL;
    X4 AT %IX1.10.4 : BOOL;
  END_VAR
  VAR
    MC_ReadActualPosition0 : MC_ReadActualPosition;
    proc_state0 : proc_state;
    MC_SetPosition0 : MC_SetPosition;
    MC_SetPosMODE : MC_EXECUTION_MODE := mcImmediately;
    MC_Home2 : MC_Home;
    MC_MoveAbsolute1 : MC_MoveAbsolute;
    MC_Power1 : MC_Power;
    MC_ReadActualPosition1 : MC_ReadActualPosition;
    proc_state1 : proc_state;
    MC_SetPosition1 : MC_SetPosition;
    MC_Home3 : MC_Home;
    MC_MoveAbsolute2 : MC_MoveAbsolute;
    MC_Power2 : MC_Power;
    MC_ReadActualPosition2 : MC_ReadActualPosition;
    proc_state2 : proc_state;
    MC_SetPosition2 : MC_SetPosition;
    WK_PID1 : WK_PID;
    WK_PID2 : WK_PID;
    WK_PID3 : WK_PID;
    MC_Home1 : MC_Home;
    MC_MoveAbsolute3 : MC_MoveAbsolute;
    MC_Power3 : MC_Power;
    MC_ReadActualPosition3 : MC_ReadActualPosition;
    proc_state3 : proc_state;
    MC_SetPosition3 : MC_SetPosition;
    MC_WriteParameter0 : MC_WriteParameter;
    MC_WriteParameter1 : MC_WriteParameter;
    MC_WriteParameter2 : MC_WriteParameter;
    MC_WriteParameter3 : MC_WriteParameter;
    MC_WriteParameter4 : MC_WriteParameter;
    MC_WriteParameter5 : MC_WriteParameter;
    MC_WriteParameter6 : MC_WriteParameter;
    MC_WriteParameter7 : MC_WriteParameter;
    MC_WriteParameter8 : MC_WriteParameter;
    MC_WriteParameter9 : MC_WriteParameter;
    MC_WriteParameter10 : MC_WriteParameter;
    MC_WriteParameter11 : MC_WriteParameter;
    WK_HYP1 : WK_HYP;
    WK_HYP2 : WK_HYP;
    WK_HYP4 : WK_HYP;
    WK_HYP0 : WK_HYP;
    stepper3 : stepper;
    stepper0 : stepper;
    stepper1 : stepper;
    stepper2 : stepper;
    _TMP_ADD221_OUT : REAL;
    _TMP_ADD103_OUT : REAL;
    _TMP_ADD223_OUT : REAL;
    _TMP_ADD227_OUT : REAL;
    _TMP_REAL_TO_UINT31_OUT : UINT;
    _TMP_REAL_TO_UINT25_OUT : UINT;
    _TMP_REAL_TO_UINT63_OUT : UINT;
    _TMP_REAL_TO_UINT96_OUT : UINT;
    _TMP_SUB200_OUT : REAL;
    _TMP_SUB185_OUT : REAL;
    _TMP_SUB215_OUT : REAL;
    _TMP_SUB177_OUT : REAL;
  END_VAR

  _TMP_ADD221_OUT := ADD(TEMP_IN1, JIAOZHUN1);
  TEMP1 := _TMP_ADD221_OUT;
  _TMP_ADD103_OUT := ADD(TEMP_IN2, JIAOZHUN2);
  TEMP2 := _TMP_ADD103_OUT;
  _TMP_ADD223_OUT := ADD(TEMP_IN3, JIAOZHUN3);
  TEMP3 := _TMP_ADD223_OUT;
  _TMP_ADD227_OUT := ADD(TEMP_IN4, JIAOZHUN4);
  TEMP4 := _TMP_ADD227_OUT;
  stepper1(AXIS := AXIS_1, TARGET := TARGET2, RUN := RUN2, SPEED := SPEED2, ASPEED := ASPEED2, HOME := HOME2, HOMESPEED := HOMESPEED2, HOMELOOKUP := HOMESPEEDZERO2, XIFEN := XIFEN2, RUN_DIR := DIR_P, HOMEMODE := HOME_P);
  POS2 := stepper1.POS;
  stepper0(AXIS := AXIS_0, TARGET := TARGET1, RUN := RUN1, SPEED := SPEED1, ASPEED := ASPEED1, HOME := HOME1, HOMESPEED := HOMESPEED1, HOMELOOKUP := HOMESPEEDZERO1, XIFEN := XIFEN1, RUN_DIR := DIR_P, HOMEMODE := HOME_P);
  POS1 := stepper0.POS;
  STATUS2 := stepper1.STATE;
  STATUS1 := stepper0.STATE;
  HOME2 := stepper1.HOME;
  HOME1 := stepper0.HOME;
  stepper2(AXIS := AXIS_2, TARGET := TARGET3, RUN := RUN3, SPEED := SPEED3, ASPEED := ASPEED3, HOME := HOME3, HOMESPEED := HOMESPEED3, HOMELOOKUP := HOMESPEEDZERO3, XIFEN := XIFEN3, RUN_DIR := DIR_P, HOMEMODE := HOME_P);
  POS3 := stepper2.POS;
  stepper3(AXIS := AXIS_3, TARGET := TARGET4, RUN := RUN4, SPEED := SPEED4, ASPEED := ASPEED4, HOME := HOME4, HOMESPEED := HOMESPEED4, HOMELOOKUP := HOMESPEEDZERO4, XIFEN := XIFEN4, RUN_DIR := DIR_P, HOMEMODE := HOME_P);
  POS4 := stepper3.POS;
  STATUS3 := stepper2.STATE;
  STATUS4 := stepper3.STATE;
  HOME3 := stepper2.HOME;
  HOME4 := stepper3.HOME;
  WK_HYP2(EN := WKON4, H := WKSET4, CV := TEMP4, L := _TMP_SUB200_OUT);
  IF WK_HYP2.ENO THEN
      PWM4 := WK_HYP2.OUT;
  END_IF;
  WK_HYP0(EN := WKON2, H := WKSET2, CV := TEMP2, L := _TMP_SUB177_OUT);
  IF WK_HYP0.ENO THEN
      PWM2 := WK_HYP0.OUT;
  END_IF;
  WK_HYP1(EN := WKON3, H := WKSET3, CV := TEMP3, L := _TMP_SUB185_OUT);
  IF WK_HYP1.ENO THEN
      PWM3 := WK_HYP1.OUT;
  END_IF;
  WK_HYP4(EN := WKON1, H := WKSET1, CV := TEMP1, L := _TMP_SUB215_OUT);
  IF WK_HYP4.ENO THEN
      PWM1 := WK_HYP4.OUT;
  END_IF;
  WK_PID0(EN := WKON1, SV := WKSET1, CV := TEMP1, RANGER := 100.0, KP := KP, KI := KI, KD := KD);
  WK_PID1(EN := WKON2, SV := WKSET2, CV := TEMP2, RANGER := 100.0, KP := KP, KI := KI, KD := KD);
  _TMP_REAL_TO_UINT31_OUT := REAL_TO_UINT(WK_PID0.OUT);
  _TMP_REAL_TO_UINT25_OUT := REAL_TO_UINT(WK_PID1.OUT);
  WK_PID2(EN := WKON3, SV := WKSET3, CV := TEMP3, RANGER := 100.0, KP := KP, KI := KI, KD := KD);
  WK_PID3(EN := WKON4, SV := WKSET4, CV := TEMP4, RANGER := 100.0, KP := KP, KI := KI, KD := KD);
  _TMP_REAL_TO_UINT63_OUT := REAL_TO_UINT(WK_PID2.OUT);
  _TMP_REAL_TO_UINT96_OUT := REAL_TO_UINT(WK_PID3.OUT);
  _TMP_SUB200_OUT := SUB(WKSET4, WKSET_L);
  _TMP_SUB185_OUT := SUB(WKSET3, WKSET_L);
  _TMP_SUB215_OUT := SUB(WKSET1, WKSET_L);
  _TMP_SUB177_OUT := SUB(WKSET2, WKSET_L);
END_PROGRAM


CONFIGURATION config

  RESOURCE resource1 ON PLC
    TASK task0(INTERVAL := T#10ms,PRIORITY := 0);
    PROGRAM instance0 WITH task0 : program0;
  END_RESOURCE
END_CONFIGURATION
